﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Skill.Ode;
using Skill.Ode.Extension;
using Skill.Ode.Geometries;
using Skill.Ode.Joints;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Samples.Common;

namespace Samples.Demos
{
    public class Chain2 : OdeDemo
    {
        /* some constants */

        private const int NUM = 10;			    /* number of boxes      */
        private const float SIDE = 0.2f;		/* side length of a box */
        private const float MASS = 1.0f;		/* mass of a box        */
        private const float RADIUS = 0.1732f;	/* sphere radius        */


        /* dynamics and collision objects */
        private Ode ode;
        private DSimpleSpace space;
        private DBall[] joint = new DBall[9];
        private MovableEntity<DBox>[] boxes = new MovableEntity<DBox>[NUM];
        private DNearCallback callback;
        private double angle = 0;
        private DContactInfoArray contact;

        #region Constructor
        public Chain2(DemoFramework framework)
            : base(framework)
        {
            callback = new DNearCallback(NearCallback);
        }
        #endregion

        //this is called by dSpaceCollide when two objects in space are
        //potentially colliding.       
        private void NearCallback(object data, DSpace o1, DSpace o2)
        {
            /* exit without doing anything if the two bodies are connected by a joint */
            DBody b1, b2;
            b1 = ((DGeometry)o1).Body;
            b2 = ((DGeometry)o2).Body;
            if (b1 != null && b2 != null && DJoint.AreConnected(b1, b2)) return;

            contact[0].Surface.Mode = 0;
            contact[0].Surface.Mu = float.MaxValue;
            if (DGeometry.Collide(o1, o2, 1, contact) > 0)
            {
                DContact c = new DContact(ode.World, ode.Group, contact[0]);
                c.Attach(b1, b2);
            }
        }

        public override void StepDemo(Microsoft.Xna.Framework.GameTime gameTime)
        {
            angle += 0.05;
            boxes[NUM - 1].Body.AddForce(0, (float)(1.5 * (Math.Sin(angle) + 1.0)), 0);

            space.NearCallback = callback;
            space.Collide(null);
            ode.World.Step(0.05f);

            /* remove all contact joints */
            ode.Group.Empty();
            for (int i = 0; i < boxes.Length; i++)
            {
                boxes[i].UpdateDisplay();
            }
        }

        public override void Initialize()
        {
            angle = 0;
            Framework.Camera.Position = new Vector3(1, 1, 5);
            contact = new DContactInfoArray(1);

            /* create world */
            ode = new Ode();
            ode.World.CFM = 1.0f / 100000.0f;
            ode.World.Gravity = new DVector3(0, -0.5f, 0);

            space = new DSimpleSpace(null);            
            DPlane groundPlane = new DPlane(space, 0, 1, 0, 0);
            DisplayBox displayGround = new DisplayBox(Framework.GraphicsDevice, Color.LightGray, Matrix.CreateTranslation(0, -0.05f, 0), new Vector3(100, 0.1f, 100));
            Framework.Add(displayGround);
            DMass m = new DMass();
            m.SetBox(1, SIDE, SIDE, SIDE);
            m.Adjust(MASS);
            for (int i = 0; i < NUM; i++)
            {
                boxes[i] = new MovableEntity<DBox>(ode.World, new DBox(space, SIDE, SIDE, SIDE));
                float k = i * SIDE;
                boxes[i].Body.Position = new DVector3(k, k + 0.4f, k);
                boxes[i].Body.Mass = m;
                boxes[i].Display = new DisplayBox(Framework.GraphicsDevice, Color.Gold, new Vector3(SIDE, SIDE, SIDE));
                Framework.Add(boxes[i].Display);
            }
            for (int i = 0; i < (NUM - 1); i++)
            {
                joint[i] = new DBall(ode.World, null);
                joint[i].Attach(boxes[i].Body, boxes[i + 1].Body);
                float k = (i + 0.5f) * SIDE;
                joint[i].Anchor1 = new DVector3(k, k + 0.4f, k);
            }
        }

        public override void Quit()
        {
            foreach (MovableEntity<DBox> b in boxes)
            {
                b.Dispose();
            }
            foreach (DBall b in joint)
            {
                b.Dispose();
            }
            this.contact.Dispose();
            space.Dispose();
            ode.Dispose();
        }
    }
}
